#include "MAX30102.h"
#include <algorithm>
#include <cmath>
bool MAX30102::GetInterruptStatus(uint8_t Flag)
{
	uint8_t buf = (Flag == IT_Internal_Temperature_Ready) ? INTERRUPT_STATUS_2 : INTERRUPT_STATUS_1;
	Port.Read(Address | buf, &buf, 1); //读标志位
	return (bool)(buf & Flag);
}

void MAX30102::SetInterrupt(Interrupt_t Flag, bool State)
{
	uint8_t buf, Addr = (Flag == IT_Internal_Temperature_Ready) ? INTERRUPT_ENABLE_2 : INTERRUPT_ENABLE_1;
	Port.Read(Address | Addr, &buf, 1); //读
	if(State)//开启
		buf |= Flag;//改
	else//关闭
		buf &= (~Flag);//改
	Port.Write(Address | Addr, &buf, 1); //写
}

void MAX30102::SetSampleAveraging(Average_t Average)
{
	uint8_t buf, ave = Average << 5;
	Port.Read(Address | FIFO_CONFIGURATION, &buf, 1); //读
	buf &= 0x1F; //0001 1111清除高三位,改
	buf |= ave; //设置高三位
	Port.Write(Address | FIFO_CONFIGURATION, &buf, 1); //写
}

void MAX30102::SetFIFORolls(bool State)
{
	uint8_t buf;
	Port.Read(Address | FIFO_CONFIGURATION, &buf, 1);
	if(State)
		buf |= 0X10;
	else
		buf &= 0xEF;
	Port.Write(Address | FIFO_CONFIGURATION, &buf, 1);
}

void MAX30102::SetFIFOAlmostFullValue(uint8_t Value)
{
	uint8_t buf;
	Port.Read(Address | FIFO_CONFIGURATION, &buf, 1);
	buf &= 0xF0;
	buf |= (Value & 0x0F);
	Port.Write(Address | FIFO_CONFIGURATION, &buf, 1);
}

BasicIO::Resault MAX30102::SetShutDown(bool State)
{
	uint8_t buf;
	auto res = BasicIO::Fail;
	res = Port.Read(Address | MODE_CONFIGURATION, &buf, 1);
	if (res != BasicIO::Succ)
		return res;
	if (State)
		buf |= 0x80;
	else
		buf &= 0x7F;
	return Port.Write(Address | MODE_CONFIGURATION, &buf, 1);
}

BasicIO::Resault MAX30102::SetMode(Mode_t Mode)
{
	uint8_t buf;
	auto res = BasicIO::Fail;
	res = Port.Read(Address | MODE_CONFIGURATION, &buf, 1);
	if (res != BasicIO::Succ)
		return res;
	buf &= 0xF8;
	buf |= Mode;
	return Port.Write(Address | MODE_CONFIGURATION, &buf, 1);
}

void MAX30102::SetSpO2FullScale(Scale_t Scale)
{
	uint8_t buf;
	Port.Read(Address | SPO2_CONFIGURATION, &buf, 1);
	buf &= 0x9F;
	buf |= Scale << 5;
	Port.Write(Address | SPO2_CONFIGURATION, &buf, 1);
}

void MAX30102::SetSampleRate(SampleRate_t SampleRate)
{
	uint8_t buf;
	Port.Read(Address | SPO2_CONFIGURATION, &buf, 1);
	buf &= 0xE3;
	buf |= SampleRate << 2;
	Port.Write(Address | SPO2_CONFIGURATION, &buf, 1);
}

void MAX30102::SetDataWidth(Width_t Width)
{
	uint8_t buf;
	Port.Read(Address | SPO2_CONFIGURATION, &buf, 1);
	buf &= 0xFC;
	buf |= Width;
	Port.Write(Address | SPO2_CONFIGURATION, &buf, 1);
}

void MAX30102::SetConversionList(const Led_t (&list)[4])
{
	uint8_t buf;
	buf = list[0] | list[1] << 4;
	Port.Write(Address | MULTI_LED_MODE_CONTROL_REGISTERS1, &buf, 1);
	buf = list[2] | list[3] << 4;
	Port.Write(Address | MULTI_LED_MODE_CONTROL_REGISTERS2, &buf, 1);
}

float MAX30102::GetTemperature()
{
	signed char Integer, Fraction;
	Port.Read(Address | DIE_TEMP_INTEGER, &Integer, 1);
	Port.Read(Address | DIE_TEMP_FRACTION, &Fraction, 1);
	return (float)(Integer + Fraction * 0.0625);
}

void MAX30102::DeInit()
{
	uint8_t buf;
	buf = INTERRUPT_ENABLE_1_VALUE;
	Port.Write(Address | INTERRUPT_ENABLE_1, &buf, 1);
	buf = INTERRUPT_ENABLE_2_VALUE;
	Port.Write(Address | INTERRUPT_ENABLE_2, &buf, 1);

	buf = FIFO_CONFIGURATION_VALUE;
	Port.Write(Address | FIFO_CONFIGURATION, &buf, 1);
	buf = MODE_CONFIGURATION_VALUE;
	Port.Write(Address | MODE_CONFIGURATION, &buf, 1);
	buf = SPO2_CONFIGURATION_VALUE;
	Port.Write(Address | SPO2_CONFIGURATION, &buf, 1);

	buf = LED1_AMPLITUDE_CONFIGURATION_VALUE;
	Port.Write(Address | LED1_AMPLITUDE_CONFIGURATION, &buf, 1);
	buf = LED2_AMPLITUDE_CONFIGURATION_VALUE;
	Port.Write(Address | LED2_AMPLITUDE_CONFIGURATION, &buf, 1);
	buf = PROXIMITY_MODE_LED_PULSE_AMPLITUDE_VALUE;
	Port.Write(Address | PROXIMITY_MODE_LED_PULSE_AMPLITUDE, &buf, 1);
	buf = MULTI_LED_MODE_CONTROL_REGISTERS1_VALUE;
	Port.Write(Address | MULTI_LED_MODE_CONTROL_REGISTERS1, &buf, 1);
	buf = MULTI_LED_MODE_CONTROL_REGISTERS2_VALUE;
	Port.Write(Address | MULTI_LED_MODE_CONTROL_REGISTERS2, &buf, 1);


	buf = PROXIMITY_INTERRUPT_THRESHOLD_VALUE;
	Port.Write(Address | PROXIMITY_INTERRUPT_THRESHOLD, &buf, 1);

	Port.Read(Address | INTERRUPT_STATUS_1, &buf, 1);
	Port.Read(Address | INTERRUPT_STATUS_2, &buf, 1);
}

bool MAX30102::AdjustScale()
{
	uint32_t FIFOBuf[32] = {0};//缓冲采样数据
	Scale_t ScaleNow = Scale_2048;
	while(true)
	{
		this->SetSpO2FullScale(ScaleNow);
		while(not this->GetInterruptStatus(IT_FIFO_Almost_Full));
		for(auto &i : FIFOBuf)
		{
			uint8_t buf[6];
			this->GetFIFOData(&buf, 6); //采样数据
			//i = buf[0] << 16 | buf[1] << 8 | buf[2]; //合成数据
			i = buf[3] << 16 | buf[4] << 8 | buf[5]; //合成数据
			Debug.Printf("GetData:%d(%#02X)\r\n", i, i);
		}
		std::sort(FIFOBuf, FIFOBuf + 32); //升序排序
		switch(ScaleNow)//根据不同的量程做不同的应对
		{
			case Scale_2048:
				Debug.Printf("Now Scale_2048\r\nMaxValue:%d\r\nMinValue:%d\r\n\r\n", FIFOBuf[31], FIFOBuf[0]);
				if(FIFOBuf[31] >= 2047) //采样值饱和
					ScaleNow = Scale_4096;//切换至更大量程
				else if(FIFOBuf[0] < 512) //采样值不足量程的1/4
					return false;
				else//符合要求的量程
					return true;
				break;

			case Scale_4096:
				Debug.Printf("Now Scale_4096\r\nMaxValue:%d\r\nMinValue:%d\r\n\r\n", FIFOBuf[31], FIFOBuf[0]);
				if(FIFOBuf[31] >= 4095) //采样值饱和
					ScaleNow = Scale_8192;//切换至更大量程
				else if(FIFOBuf[0] < 1024) //采样值不足量程的1/4
					ScaleNow = Scale_2048;//切换至更小量程
				else//符合要求的量程
					return true;
				break;

			case Scale_8192:
				Debug.Printf("Now Scale_8192\r\nMaxValue:%d\r\nMinValue:%d\r\n\r\n", FIFOBuf[31], FIFOBuf[0]);
				if(FIFOBuf[31] >= 8191) //采样值饱和
					ScaleNow = Scale_16384;//切换至更大量程
				else if(FIFOBuf[0] < 2048) //采样值不足量程的1/4
					ScaleNow = Scale_4096;//切换至更小量程
				else//符合要求的量程
					return true;
				break;

			case Scale_16384:
				Debug.Printf("Now Scale_16384\r\nMaxValue:%d\r\nMinValue:%d\r\n\r\n", FIFOBuf[31], FIFOBuf[0]);
				if(FIFOBuf[31] >= 16384) //采样值饱和
					return false;
				else if(FIFOBuf[0] < 4096) //采样值不足量程的1/4
					ScaleNow = Scale_8192;//切换至更小量程
				else//符合要求的量程
					return true;
				break;
		}//switch end
	}//while(true) end
}

bool MAX30102::AutoScale()
{
	uint8_t Strength = 0;
	while((not this->AdjustScale()) and (Strength < 255))
	{
		Debug.Printf("Now Strength:%d\r\n", Strength);
		this->SetLEDAmplitude(1, Strength);
		this->SetLEDAmplitude(2, Strength);
		Strength++;
	}
	Debug.Printf("Best Strength:%d\r\n", Strength);
	return Strength < 255 ? true : false;
}

uint8_t MAX30102::GetRevID()
{
	uint8_t ret = 0;
	Port.Read(Address | REVISION_ID, &ret, 1);
	return ret;
}
uint8_t MAX30102::GetPartID()
{
	uint8_t ret = 0;
	Port.Read(Address | PART_ID, &ret, 1);
	return ret;
}

void MAX30102::RegDump()
{
	uint8_t buf[22], *pb = buf;
	const char* str[22] =
	{
		"INTERRUPT_STATUS_1",
		"INTERRUPT_STATUS_2",
		"INTERRUPT_ENABLE_1",
		"INTERRUPT_ENABLE_2",
		"FIFO_WRITE_POINTER",
		"FIFO_OVERFLOW_COUNTER",
		"FIFO_READ_POINTER",
		"FIFO_DATA",
		"FIFO_CONFIGURATION",
		"MODE_CONFIGURATION",
		"SPO2_CONFIGURATION",
		"LED1_AMPLITUDE_CONFIGURATION",
		"LED2_AMPLITUDE_CONFIGURATION",
		"PROXIMITY_MODE_LED_PULSE_AMPLITUDE",
		"MULTI_LED_MODE_CONTROL_REGISTERS1",
		"MULTI_LED_MODE_CONTROL_REGISTERS2",
		"DIE_TEMP_INTEGER",
		"DIE_TEMP_FRACTION",
		"DIE_TEMPERATURE_CONFIG",
		"PROXIMITY_INTERRUPT_THRESHOLD",
		"REVISION_ID",
		"PART_ID"
	};
	Port.Read(Address | INTERRUPT_STATUS_1, pb, 8);
	pb += 8;

	Port.Read(Address | FIFO_CONFIGURATION, pb++, 1);
	Port.Read(Address | MODE_CONFIGURATION, pb++, 1);
	Port.Read(Address | SPO2_CONFIGURATION, pb++, 1);
	Port.Read(Address | LED1_AMPLITUDE_CONFIGURATION, pb++, 1);
	Port.Read(Address | LED2_AMPLITUDE_CONFIGURATION, pb++, 1);
	Port.Read(Address | PROXIMITY_MODE_LED_PULSE_AMPLITUDE, pb++, 1);
	Port.Read(Address | MULTI_LED_MODE_CONTROL_REGISTERS1, pb++, 1);
	Port.Read(Address | MULTI_LED_MODE_CONTROL_REGISTERS2, pb++, 1);

	Port.Read(Address | DIE_TEMP_INTEGER, pb++, 1);
	Port.Read(Address | DIE_TEMP_FRACTION, pb++, 1);
	Port.Read(Address | DIE_TEMPERATURE_CONFIG, pb++, 1);

	Port.Read(Address | PROXIMITY_INTERRUPT_THRESHOLD, pb++, 1);

	Port.Read(Address | REVISION_ID, pb++, 1);
	Port.Read(Address | PART_ID, pb++, 1);
	for(int i = 0; i < 22; i++)
		Debug.Printf("%s:%#2X\r\n", str[i], buf[i]);
}

